#include <list>
#include <vector>
#include <algorithm>

#include "../math/mathbase.h"
#include "../sdf/dynarray.h"
#include "mact.h"
#include "pqueue.h"

using namespace PBVP;

//const static float VOL_THRESHOLD = 0.0001;

static float VOL_THRESHOLD = 0.01f;

MultiConTree::MultiConTree(DualGraph *pDCTree)
{
	// we assume max_range is power of 2
	int nrange = pDCTree->numOfRanges();
	m_MaxLevel = Math::log2(nrange);
	printf("range # = %d, level # = %d\n", nrange, m_MaxLevel);
	m_DualTrees = (DualGraph **)malloc(sizeof(DualGraph *)*(m_MaxLevel+1));
	m_DualTrees[m_MaxLevel] = pDCTree;
	
	m_PruneThreshold = 0.01f;

	// create higher level DCT by merging ranges
	int i, j, k;
#ifdef _DEBUG_MACT
	m_DualTrees[i]->print();
#endif
	for (i = m_MaxLevel; i > 0; i--) {
		m_DualTrees[i-1] = m_DualTrees[i]->mergeRanges();
#ifdef _DEBUG_MACT
		printf("dual tree at level %d\n", i-1);
		m_DualTrees[i-1]->print();
#endif
	}
	// total volume of interval volumes
	float volSum = 0;
	for (i = 0; i < m_DualTrees[0]->numOfNodes(0); i++) {
		volSum += getNodePtr(0, 0, i)->getVolume(); 
	}
	
	fprintf(stdout, "Total Volume of the DCT: %f \n", volSum);
	for (i = 0; i <= m_MaxLevel; i++) {
        for (j = 0; j < m_DualTrees[i]->numOfRanges(); j++) {
			for (k = 0; k < m_DualTrees[i]->numOfNodes(j); k++) {
				// normalize volume
				getNodePtr(i, j, k)->normVolume = getNodePtr(i, j, k)->getVolume() / volSum;
				// getNodePtr(i, j, k)->fint /= volSum;
			}
		}
	}

	prune(m_PruneThreshold);

#ifdef OUTPUT_PRUNE
	for(int lev = m_MaxLevel; lev >= 0; lev --) {
		char treeFile[256];
		sprintf(treeFile, "pruned_lev_%d.dct\0", lev);
		m_DualTrees[lev]->saveFile(treeFile);
	}
	//printf("level 0 dual tree after pruning\n");
#endif

#ifdef _COMPARE
	printf("ID \t Volume\t \t Moment of Inertia \t\t Pot Integral \t Dipole \t Moment of Quadrupole\n");
	for(i = 0; i <= 1; i++) {
		printf("----------------level %d------------------------\n", i);
		for(j = 0; j < m_DualTrees[i]->numOfRanges(); j++) {
			for(k = 0; k < m_DualTrees[i]->numOfNodes(j); k++) {
				if(m_DualTrees[i]->node(j, k)->normVolume >= 0.1*m_DualTrees[i]->numOfRanges()) {
					m_DualTrees[i]->node(j, k)->printPretty();
				}
			}
		}
	}
#else
	m_DualTrees[0]->print(); 
#endif
	// sort nodes by their volumes
	FILE* log = fopen("DCT.log", "w");
	std::vector<float> volsize;
	for(i = 0; i < m_DualTrees[0]->numOfNodes(0) ; i++) {
		volsize.push_back((float) ( getNodePtr(0, 0, i)->getVolume()));
	}
	std::sort(volsize.begin(), volsize.end());
	for(i = 1, j = volsize.size(); j > 0 ; j--, i++) {
		fprintf(log, "%d %f\n", i, volsize[j-1]);
	}	
	fclose(log); 
}

void MultiConTree::prune(float threshold)
{
	m_DualTrees[0]->prune(threshold);

	// prune DCT at finer resoultions
	for(int level = 1; level <= m_MaxLevel; level++) {
		m_DualTrees[level]->prune(threshold);
	}
}

QString MultiConTree::toQString() const
{
	return m_DualTrees[0]->toQString();
}

MultiConTree::~MultiConTree()
{
	for (int i = 0; i <= m_MaxLevel; i++) {
		delete m_DualTrees[i];
	}
	free(m_DualTrees); 
}


/*void MultiConTree::getPCS(int l, int r, float *ctr, float **axes)
{
	assert(l <= max_level && r < dual_trees[l]->numOfRanges());

	float *eigval = fvector(1, 3);
	float **eigvec = matrix(1, 3, 1, 3);
	VolMoments mom;
	for (int k = 0; k < dual_trees[l]->nNodes(r); k++) {
		mom = mom + dual_trees[l]->node(r, k)->moments;
	}	
	mom.print();
	mom.getCOM(ctr);
	mom.principalAxes(eigval, eigvec);
	for(int i = 0; i < 3; i++) {
		for(int j = 0; j < 3; j++) {
			axes[i][j] = eigvec[i+1][j+1];
		}
	}
	free_vector(eigval, 1, 3);
	free_matrix(eigvec, 1, 3, 1, 3);
}*/


SimScore MultiConTree::match(MultiConTree &mact2)
{
	SimScore simscore;
	MultiConTree& mact1 = *this;
	int level = std::min(mact1.maxLevel(), mact2.maxLevel());
	// create a matched list for each level
	std::list<DualNode *>* matched_lists = new std::list<DualNode *>[level+1];

	int i, j, k;
	PriorityQueue<DualNode *> *que = new PriorityQueue<DualNode *>;
	DualNode *pNode;

	// Normalized volume of the entire dual tree
	float tvol[2];

	tvol[0] = mact1.totalVolume();
	tvol[1] = mact2.totalVolume();

	// First try to match the level 0
	for (i = 0; i < mact1.m_DualTrees[0]->numOfNodes(0); i++) {
		pNode = mact1.m_DualTrees[0]->node(0, i);
		que->insert(pNode, pNode->importance());
	}
	while (!que->isEmpty()) {
		que->extract(pNode);
		DualNode *pMatch = mact1.matchSearch(pNode, mact2.m_DualTrees[0]);
		if (pMatch != NULL) {
			pNode->setMatchNode(pMatch);
			matched_lists[0].push_back(pNode);
			// printf("node %d matched with node %d\n", pNode->id, pMatch->id);
		}
	}
    // consider the rest levels
	for (i = 1; i <= level; i++) {
		// iterate for each range 
		for (j = 0; j < mact1.m_DualTrees[i]->numOfRanges(); j++) {
			// iterate for nodes in a range j
			for (k = 0; k < mact1.m_DualTrees[i]->numOfNodes(j); k++) {
				pNode = mact1.m_DualTrees[i]->node(j, k);
				if (pNode->parent->isMatched())
					que->insert(pNode, pNode->importance());
			}
		}
		while (!que->isEmpty()) {
			que->extract(pNode); 
			DualNode *pMatch = mact1.matchSearch(pNode, mact2.m_DualTrees[i]);
			// match found
			if (pMatch != NULL) {
				pNode->setMatchNode(pMatch);
				matched_lists[i].push_back(pNode);
			}
		}
	}
	delete que;

	// compute the similarity score between two multi-res con trees
	for (i = 0; i <= level; i++) {
		// printf("---------------------------------level %d scores---------------------------\n", i);
		while (!matched_lists[i].empty()) {
			pNode = matched_lists[i].front();
			matched_lists[i].pop_front();
            // printf("node %d of MACT 1 matchs node %d of MACT2\n", pNode->id, pNode->match->id);
			SimScore sim = pNode->similarity(pNode->match);
			//if(i == 0) {
			//	printf("node %d has geoScore %f with normVol = %f\n", pNode->id, sim.geoScore, pNode->normVolume);
			//}
			simscore += sim * ((pNode->normVolume + pNode->match->normVolume) / 2);
		}
		
	}
	
	simscore = simscore / ((level+1)*std::max(tvol[0], tvol[1]));

	delete[] matched_lists;
	mact1.clearMatchs();
	mact2.clearMatchs();
	return simscore;	
}

/************************************************************************/
/* Private functions                                                    */
/************************************************************************/


DualNode* MultiConTree::matchSearch(DualNode* dualnode, DualGraph* graph)
{
	int i, lvl = dualnode->range;
	PriorityQueue<DualNode *> candidates;
	DualNode* mtch_node = NULL;
	float grade;
	for (i = 0; i < graph->numOfNodes(lvl); i++) {
		DualNode *p_node = graph->node(lvl, i);
		if (p_node->isParentMatched(dualnode) && !(p_node->isMatched())) {
			candidates.insert(graph->node(lvl, i), graph->node(lvl, i)->importance());
			//printf("*************candidtate: %d %f\n", (graph->nodes[lvl])[i]->id, (graph->nodes[lvl])[i]->weight());
		}
	}
	while (!candidates.isEmpty()) {
		DualNode *cand;
		candidates.extract(cand);
		float scor = matchScore(dualnode, cand);
		if (mtch_node == NULL) {
			grade = scor;
			mtch_node = cand;
		} else if (scor > grade) {
			grade = scor;
			mtch_node = cand;
		}
	}
	return mtch_node;
}

float MultiConTree::matchScore(DualNode *node1, DualNode *node2)
{
	float s;
	s = node1->simpleScore(node2);
	return s;
}
